COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.NEWSEC PROGRAMMING EXAMPLES,BOLTING A BRACKET
C00007 00003	.ex1: NEWSSS EXAMPLE ONE
C00019 00004	.NEWSSS EXAMPLE TWO
C00029 00005	.NEWSSS EXAMPLE THREE
C00044 00006
C00048 00007	.NEWSS EXAMPLES OF COORDINATED ACTION, COORDINATED ACTION
C00057 00008	.NEWSS A α`VERY HIGH LEVELα' EXAMPLE
C00063 ENDMK
C⊗;
.NEWSEC PROGRAMMING EXAMPLES,BOLTING A BRACKET
.NEWSS BOLTING A BRACKET ONTO A BEAM, BOLTING A BRACKET

This  is intended  to  be  a  series of  progressively  more  complex
examples  which demonstrate  some of the  features in  HAL, including
attachment, control structure,  macros, and library routines.  All of
the examples have  essentially the same goal: bolt a  BRACKET to a beam.
Each  example takes  into  account more  possibilities or  contains a
different way of expressing the same thing.

The initial and final attachment structures are:
.NOFILL

        INITIAL: TABLE		      FINAL: TABLE
		    YELLOW			YELLOW
		    BLUE			BLUE
		 BRACKET		     BEAM
		    BRACKET_HOLE		BEAM_HOLE
		    BRACKET_GRASP		BRACKET
		 BOLT				    BRACKET_HOLE
		 BEAM				    BRACKET_GRASP
		    BEAM_HOLE			BOLT

.FILL

The initial structure  can be created by the  following declarations.
There will eventually be a way of interacting with the HAL supervisor
to  initialize  frame   values,    either   by  reading  an   "object
description" (which might have come from the designer) or by manually
positioning  the arm at the  desired positions.
See {NEWFIG Initial World,FULL}.
.NOFILL

UNITS CENTIMETERS, BOLLES, DEGREES, SECONDS;
.COMT 4
	 COMMENT this sets the defaults to the indicated units.  Notice
	that BOLLES angles mean a rotation about the X Axis, followed
	by a rotation about the original Y axis, followed by a final
	rotation about the original Z axis;
.END

FRAME BEAM, BEAM_HOLE;
FRAME BRACKET, BRACKET_HOLE, BRACKET_GRASP;
FRAME BOLT;
BEAM α← [(30, 24.2, 0) : (0, 0, 90)];
.COMT 4
    COMMENT this is not attached to anything initially.  Thus the
	default DEPROACH is the TABLE's DEPROACH which is:
	[(0, 0, 10) | (0, 0, 0)];
.END

BEAM_HOLE α← BEAM * [(5.1, 0, 15) : (-90, 0, 0)];
.COMT  4
    COMMENT [(5.1, 0, 15):(90, 0, 90)] is the relative transform from
	BEAM to the BEAM_HOLE.  Another way of looking at this is that
	within the BEAM's frame of reference, the BEAM_HOLE is at
	[(5.1, 0, 15):(90, 0, 90)].  The premultiplication by BEAM transforms
	this relative location out to the corresponding position (in TABLE
	coordinates) with respect to the current location of BEAM;
.END

ATTACH BEAM_HOLE TO BEAM;
ASSERT FACT (DEPROACH BEAM_HOLE [(0, 0, -3)|(0, 0, 0)]);
.COMT  4
    COMMENT this sets up a DEPROACH of -3 centimeters in the Z direction
	of the BEAM_HOLE's coordinate system;
.END

BRACKET α← [(20, 40, 0) : (0, 0, 90)];
BRACKET_HOLE α← BRACKET * [(5.1, 2, 0) : (180, 0, 0)];
ATTACH BRACKET_HOLE TO BRACKET;
BRACKET_GRASP α← BRACKET * [(0, 1.5, 5) : (180, 0, 0)];

ATTACH BRACKET_GRASP TO BRACKET RIGIDLY;
.COMT  4
    COMMENT notice that changing BRACKET_GRASP will automatically change
	BRACKET, which in turn will automatically change BRACKET_HOLE.  This
	is very handy if the position of the whole `object' is being
	updated by one grasping position (ie. BRACKET_GRASP);
.END
BOLT α← [(30, 60, 5) : (180, 0, 90)];
.COMT 4
    COMMENT the bolt is assumed to be sticking out of a dispenser;
.END
.ex1: NEWSSS EXAMPLE ONE

The task involves the following steps:
.BEGIN INDENT 8,12; FILL;PREFACE 0;

	(1) Pick up the BRACKET with the YELLOW arm and position
	    it next to the BEAM so that the holes line up

	(2) Pick up the bolt with the BLUE arm and insert
	    it in the hole (in this example it is not screwed
	    in; a later example will use a socket driver
	    to tighten the bolt)

	(3) Return both arms to park
.END

.FILL
The BRACKET is assumed to be 1cm thick , and the BOLT 4cm long.
The following  program  is a  straightforward way  of expressing  the
motions and feedback  necessary to carry out the task.  Everything is
assumed to  be in  the right  place and  every motion  is assumed  to
accomplish the  desired effect.  For example,   this  program assumes
that  the arm  is  accurate enough  to align  the BRACKET_HOLE  with the
BEAM_HOLE and to insert the BOLT without hitting the side or binding.
Later examples will take this type of error into account.
.NOFILL

OPERATE YFINGERS WITH OPENING=1;
MOVE YELLOW TO BRACKET_GRASP;
.comt 4
    COMMENT since BRACKET_GRASP does not have a DEPROACH explicitly
	associated with it, the compiler checks to see if it is 
	attached to anything.  It is, BRACKET.  But BRACKET does not
	have a DEPROACH associated with it either.  Is it attached
	to anything?  No.  Therefore, by default the compiler uses the TABLE's
	DEPROACH (ie. [(0, 0, 10):(0, 0, 0)] ) as the approach for
	BRACKET_GRASP;
.END

CENTER YELLOW;
.COMT 4
    COMMENT this closes the fingers until they grab something;
.END
BRACKET_GRASP α← YELLOW;
.COMT 4
   COMMENT since BRACKET_GRASP is RIGIDLY attached to BRACKET, this
	statement updates BRACKET and hence anything attached to
	BRACKET (eg. BRACKET_HOLE).  In effect, the assumption being
	made is that the position of the whole `object' (ie. the BRACKET)
	can be updated by locating BRACKET_GRASP.  In the usage above the
	arm moves to the planning position for BRACKET_GRASP and then
	centers itself about the object between its fingers.  Notice that
	the final position of the arm may very well not be BRACKET_GRASP
	(because of the accommodation within the centering).  Therefore,
	the BRACKET might not be where it was planned to be.  This discrepancy
	between the planned world and the `actual' world has to be
	reconciled.  The simplest assumption (and the assumption being used here)
	is that the only difference between the planned location and the
	actual is that the `whole' BRACKET has been moved along the line between
	the fingers so that BRACKET_GRASP is where the arm found it.  More
	complicated updating could be done by visually locating the BRACKET
	and reseting BRACKET or by feeling the BRACKET two or three times,
	combining the resulting locations into a new estimate of BRACKET's
	location, and reseting BRACKET.  Notice that if the CENTER moved the arm
	away from the planned location and no updating were done, the ATTACH
	statement which follows  would attach the BRACKET to
	the YELLOW arm in such a way that the BRACKET was assumed to be at
	its planning position (which would be wrong).  The subsequent move
	to the BEAM_HOLE would also be off by the same amount.;
.END

ATTACH BRACKET TO YELLOW;
MOVE BRACKET_HOLE TO BEAM_HOLE;
.COMT 4
    COMMENT notice that the BRACKET approaches the BEAM from the side
	(not from above) because of the DEPROACH set up for BEAM_HOLE.
	In this example the BRACKET is assumed to go right next
	to the BEAM;

    This MOVE is a move for the YELLOW arm (because the BRACKET
	is attached to it).  From the definition of attachment this
	means that anything attached to the YELLOW arm is automatically
	moved.  Thus, BRACKET, BRACKET_HOLE, and BRACKET_GRASP are all
	updated.  The fact that the move was specified by mentioning
	BRACKET_HOLE (and not YELLOW) does not change the automatic
	updating within the graph structure.  Notice, in particular,
	that this is quite different from:
.BEGIN NOFILL
		ATTACH BRACKET TO YELLOW
		BRACKET_HOLE α← BEAM_HOLE
.END
	This would change the value of BRACKET_HOLE and the relative
	position between BRACKET and BRACKET_HOLE, but leave YELLOW and BRACKET
	unchanged.	
.END

OPERATE BFINGERS WITH OPENING=1;
MOVE BLUE TO BOLT;
.COMT 4
    COMMENT the TABLE's approach is used since the BOLT is not attached
	to anything;
.END

CENTER BLUE;
BOLT α← BLUE;
.COMT 4
    COMMENT This insures that the latest value of BOLT is used in the attach
	command below;
.END

ATTACH BOLT TO BLUE;
MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE;
.COMT 4
    COMMENT this should position the bolt .3 centimeters off of the BRACKET.
	That is, the YELLOW arm is now holding the BRACKET right next to the
	BEAM (with the BRACKET_HOLE aligned with the BEAM_HOLE)  and the Blue
	arm is holding the BOLT 5.3 centimeters away from the BRACKET_HOLE
	(which is equivalent with BEAM_HOLE).  But remember that the
	BRACKET is 1 centimeter thick and the BOLT is 4 centimeters long,
	thus the tip of the BOLT is 1.3 centimeters from the BEAM_HOLE
	(or .3 off of the BRACKET).

    The following is a list of rules governing automatic DEPARTUREs and
.BEGIN INDENT 8,12
	APPROACHes:
.BREAK
	    (1) If the destination contains `⊗', no automatic departure or
		approach is used.
.BREAK
	    (2) If the destination contains only one frame name, an automatic
		approach is computed.  This is done by chaining up the
		attach structure (beginning at the frame mentioned in the
		destination) until a DEPROACH is found for one of the frames or
		until all of the frames reachable from the starting frame
		have been considered; at that point use the TABLE's
		DEPROACH, [(0,0,10):(0,0,0)].
.BREAK
	    (3) If the destination contains two or more frame names no
		automatic approach is used.
.BREAK
	    (4) In cases (2) and (3) above, an automatic departure is
		computed based upon the current position of the arm
		and how it got there.  If its current position was specified
		using only one frame name, that frames automatic deproach
		will be used for the departure when the arm moves away,
		unless an object is attached to the arm at this position.
		If an object is detached from something (say X) and attached
		to the arm in its current position, use X's DEPROACH for
		the automatic departure when the arm moves away.  This was
		included so the automatic departure does the `right' thing
		as often as possible.
.BREAK
	    (5) The automatic approaches and departures can always be
		overridden by explicit USING APPROACH = ... or
		USING DEPARTURE = ... clauses with a MOVE;
.END
.END

MOVE BLUE TO ⊗ + (0, 0, 5) WRT BEAM_HOLE
    USING FREE=(X WRT BLUE), FREE=(Y WRT BLUE)
	ON FORCE(Z WRT BLUE) > 60 DO STOP BLUE;
.COMT 4
    COMMENT the arm stops when the bolt hits the bottom of the hole.
    No deproach or approach is used because the destination
	involves the "⊗";
.END

OPERATE YFINGERS WITH OPENING = 1;
DETACH BRACKET FROM YELLOW;
ATTACH BRACKET TO BEAM;
MOVE YELLOW TO YPARK;

OPERATE BFINGERS WITH OPENING = 1;
DETACH BOLT FROM BLUE;
ATTACH BOLT TO BEAM;
MOVE BLUE TO BPARK;

WRITE("FINISHED");
.NEWSSS EXAMPLE TWO

.FILL

This version adds a number of checks  (and some automatic recoveries)
for possible run-time errors such as not inserting the BOLT.  It also
utilizes the  COBEGIN  - COEND  capability to  describe  simultaneous
(unordered,   independent) actions.   Thus,   the  Yellow arm  can be
picking  up the BRACKET  and positioning  it near the  BEAM while the
Blue arm is  picking up the BOLT.   Collision avoidance is  currently
the responsibility of the user.


.NOFILL


COBEGIN "POSITIONING"
    BEGIN "PICK UP BRACKET BY YELLOW"
    OPERATE YFINGERS WITH OPENING=1;
    MOVE YELLOW	TO BRACKET_GRASP;
    CENTER YELLOW
        ON OPENING = 0 DO 
	    BEGIN "MISSED BRACKET"
	    STOP YELLOW;
	    SCALAR FLAG;
	    OPERATE YFINGERS WITH OPENING=2;
	    MOVE YELLOW 
		TO BRACKET_GRASP * THE_DEPARTURE(BRACKET_GRASP)
		USING APPROACH = NIL, DEPARTURE = NIL;
.COMT 20
                COMMENT this should safely move the arm away so the
		    operator can easily insert the missing BRACKET.  It moves
		    the arm back out to the BRACKET_GRASP's approach point
		    at runtime.

		COMMENT the "THE_DEPARTURE(...)" is a macro which expands into
		    a DEPROACH.  This magic is described under the COMPILE-TIME
		    variables section above.  More examples are given a little
		    later in this set of examples;
.END
	    WRITE("THE BRACKET IS MISSING ... POSITION IT AND TYPE `1'
	        TO TRY AGAIN");
	    READ(FLAG);
	    IF FLAG %7≠%* 1 THEN ABORT ("YOU DID NOT TYPE 1");
.COMT 20
		COMMENT the ABORT stops everything, saves the world, and forces
		    the operator to deal with the problem at supervisor level,
		    possibly investigating the saved information, reinitializing
		    the world to some previous state and restartinc;
.END
	    MOVE YELLOW TO BRACKET_GRASP
		USING APPROACH=NIL, DEPARTURE=NIL;
.COMT 20
		COMMENT this results in a simple move without a DEPARTURE
		    or an APPROACH;
.END
	    CENTER YELLOW
		ON OPENING=0 DO ABORT("I HAVE TRIED TWICE. I GIVE UP.");
	    END "MISSED BRACKET";

    ASSERT YELLOW = #(BRACKET_GRASP);
.COMT 8
	COMMENT this tells the compiler that the Yellow arm can be
	    assumed to be at BRACKET_GRASP no matter how control got here,
	    eg. possibly moving away and retrying the grasp.  #(BRACKET_GRASP)
	    specifies the planning value of BRACKET_GRASP;
.END
    BRACKET_GRASP α← YELLOW;
.COMT 8
	COMMENT this generates code to be run at run-time which updates
	    the frame BRACKET_GRASP (which in turn updates BRACKET and BRACKET_HOLE).
	    The result is that the following attach uses the best run-time
	    value of the BRACKET's position;
.END
    ATTACH BRACKET TO YELLOW;
    MOVE BRACKET_HOLE TO BEAM_HOLE + (0, 0, 1.3) WRT BEAM_HOLE;
.COMT 8
	COMMENT this uses the TABLE's DEPARTURE (since the BRACKET is not
	    attached to anything) and the BEAM_HOLE's approach
	    (since it is the only frame mentioned in the destination).
	    This move should position the BRACKET just off of
	    the BEAM.  The next motion pushes it up against the BEAM;
.END
    MOVE YELLOW TO ⊗ + (0, 0, .5) WRT BEAM_HOLE
	ON FORCE(Z WRT BEAM_HOLE) > 50 DO STOP YELLOW
	ON ARRIVAL DO ABORT ("I SEEM TO HAVE GONE TOO FAR");
.COMT 12
	    COMMENT Give up if the expected force is not felt.
		"ARRIVAL" means that the arm reached its destination
		without being stopped by any of the
		ON-conditions.  In this case this means that the arm
		did not reach the expected force, which means that
		something went wrong.
		The STOP YELLOW disables all ON monitors for the YELLOw
		arm;
.END
    END "PICK UP BRACKET BY YELLOW";

    BEGIN "PICK UP BOLT BY BLUE"
.COMT 8
    COMMENT meanwhile the BLUE arm can be picking up the BOLT;
.END
    OPERATE BFINGERS WITH OPENING=1;
    MOVE BLUE TO BOLT;
    CENTER BLUE;  
.COMT 8
	COMMENT assume everything is OK;
.END
    BOLT α← BLUE;
    ATTACH BOLT TO TABLE;
    END "PICK UP BOLT BY BLUE"
COEND "POSITIONING";

.COMT 0
COMMENT  The BRACKET should be positioned next to the BEAM and the BLUE arm
	should be holding the BOLT;
.END

MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE
    USING THE_APPROACH(BEAM_HOLE);
.COMT 4
    COMMENT this should position the bolt .3 centimeter off of the BRACKET;
.END

.COMT 0
COMMENT now begin a search just in case the BOLT doesn't immediately go in the
	hole: make .2cm steps around in a spiral; if the BOLT does not
	go in within nine tries, abort the program;
.END
FRAME SET; SCALAR N; 
.COMT 4
    COMMENT N is the number of attempts;
.END
N α← 0;
SET α← BLUE;  
.COMT 4
    COMMENT save initial arm position;
.END
SEARCH BLUE
    INCREMENT .2;
    NORMAL_TO Z WRT BEAM_HOLE;
    REPEATING
	BEGIN "INSERTING"
	MOVE BLUE TO ⊗ + (0, 0, 1.6) WRT BEAM_HOLE
	    ON FORCE(Z WRT BEAM_HOLE) > 60 DO
		BEGIN "MISSED AGAIN"
		STOP BLUE;
		N α← N + 1;
		IF N > 9 THEN ABORT ("GIVING UP THE SEARCH");
		MOVE BLUE TO SET
		END "MISSED AGAIN"
		ON ARRIVAL DO TERMINATE;
.COMT 20
                    COMMENT this means that if the MOVE succeeds in reaching
			its goal, stop the search.  TERMINATE is a key word
			within SEARCH's;
.END
	END;

ASSERT BLUE = BEAM_HOLE + (0, 0, 3.7) WRT BEAM_HOLE;
.COMT 4
    COMMENT expect to have the bolt (which is 4 cm) .3 cm into the hole;
.END

MOVE BLUE TO ⊗ * [(0, 0, 4):(0, 0, 90)]
    USING FREE=(X WRT BLUE), FREE=(Y WRT BLUE)
    ON FORCE(Z WRT BLUE) > 60 DO STOP BLUE;
.COMT 8
            COMMENT this moves the arm 4cm straight ahead and
		twists it 90 degrees about its Z axis (ie. straight
		ahead).  Thus it moves ahead and twists;
.END

COBEGIN "DISENGAGE"
    BEGIN "YELLOW"
    OPERATE YFINGERS WITH OPENING = 1;
    DETACH BRACKET FROM YELLOW;
    ATTACH BRACKET TO BEAM;
    MOVE YELLOW TO YPARK
    END "YELLOW";

    BEGIN "BLUE"
    OPERATE BFINGERS WITH OPENING = 1;
    DETACH BOLT FROM BLUE;
    ATTACH BOLT TO BEAM;
    MOVE BLUE TO BPARK
    END "BLUE"
COEND "DISENGAGE";
WRITE("FINISHED");

.NEWSSS EXAMPLE THREE

.FILL
This example employs a  text macro to simplify definitions,   a macro
to  shorten the code for  searching,  and a  library routine to GRASP
things.   The  library  routine is  supposed  to cover  a  number  of
possibilities and provide for a  number of parameters.  Since library
routines  can be called with a subset  of their parameters filled in,
the routine's flexibility is not oppressive for those  users who just
want to do  something simple.

We begin by defining
a few little macros to change the conventions for defining macros:

.NOFILL
MACRO BEGIN_MACRO "[]" = [= ⊂];
.COMT 4
    COMMENT the "[]" is a way of specifying special macro delimiters for
	this one macro;
.END
DEFINE END_MACRO   "[]" = [⊃];

MACRO DEFINE_WRT(NEW_FRAME, MAIN_FRAME, POSITION)
    BEGIN_MACRO
    NEW_FRAME α← MAIN_FRAME * POSITION;
    ATTACH NEW_FRAME TO MAIN_FRAME;
    END_MACRO;

A typical call might be:
	DEFINE_WRT(BRACKET_HOLE, BRACKET, [(5.1, 2, 0) : (180, 0, 90)]);
which would expand into:
	BRACKET_HOLE α← BRACKET * [(5.1, 2, 0) : (180, 0, 90)];
	ATTACH BRACKET_HOLE TO BRACKET;

.FILL
Notice that  the parser is  smart enough  to distinguish between  the
commas which appear within a construct (eg. the frame) and the commas
that appear between the parameter values.

The following macro produces a string of tokens which imply a compile
time check on  the value of the conditional expanded by the parameter
RIGID.  If RIGID is a "1" (ie. true) the token sequence which rigidly
attaches the new frame to the main frame is used.
.NOFILL

MACRO DEFINE_WRT(NEW_FRAME, MAIN_FRAME, POSITION, RIGID)
    BEGIN_MACRO
    NEW_FRAME α← MAIN_FRAME * POSITION;
    COMPILE IF RIGID 
	THEN ATTACH NEW_FRAME TO MAIN_FRAME RIGIDLY
	ELSE ATTACH NEW_FRAME TO MAIN_FRAME;
    END_MACRO;

A  pair  of macros  THE_APPROACH(...)  and  THE_DEPARTURE(...)  which
expand into a compile-time statement which searches the data base for
any explicit DEPROACH associated with the parameter:

MACRO THE_DEPARTURE(X) = ⊂PICK(DEPROACH X BIND(*))⊃;
MACRO THE_APPROACH(X) = ⊂PICK(DEPROACH X BIND(*))⊃;

Another, more complicated macro to facilitate a normal search:

MACRO NORMAL_SEARCH(THE_ARM, INCREM, DIST_FWD,
    STOPPING_FORCE, NUM_TRIES)
    BEGIN_MACRO
    BEGIN COMMENT this BEGIN is part of the macro code;
	FRAME SET; SCALAR N; 
.COMT 12
	    COMMENT N is the number of attempts;
.END
	N α← 0;
	SET α← THE_ARM;  
.COMT 12
	    COMMENT save initial arm position;
.END
	SEARCH THE_ARM
	    INCREMENT INCREM
	    NORMAL_TO Z WRT THE_ARM;
	    REPEATING
		BEGIN "INSERTION"
		MOVE THE_ARM TO ⊗ + (0, 0, DIST_FWD) WRT THE_ARM
		    ON FORCE(Z WRT THE_ARM) > STOPPING_FORCE DO
		        BEGIN "MISSED AGAIN"
			STOP THE_ARM;
			N α← N + 1;
			IF N > NUM_TRIES THEN ABORT("GIVING UP");
			MOVE THE_ARM TO SET;
			END "MISSED AGAIN"
		    ON ARRIVAL DO TERMINATE;
		END "INSERTION";
	ASSERT THE_ARM = #(SET) + (0, 0, (DIST_FWD - 1));
.COMT 12
	COMMENT this changes the compiler's view to believe that the arm
		succeeds on the first attempt, BUT that it does NOT go
		as far as DIST_FWD ... possibly more realistic;
.END
    END;
    END_MACRO;

A typical call would be:
	NORMAL_SEARCH(YELLOW, .2, 1.6, 60, 9);

The above macro could easily be made into a library routine as follows:

ROUTINE NORMAL_SEARCH(FRAME THE_ARM; SCALAR INCREM, DIST_FWD,
			STOPPING_FORCE, NUM_TRIES(DEFAULT 9));
    BEGIN
	...
    END;

The corresponding call:

	NORMAL_SEARCH(YELLOW, .2, 1.6, 60, 9);

The "9" is a default value if no value is specified in the call.  Thus, by
naming the parameters the same call can be made by:

	NORMAL_SEARCH(THE_ARM=YELLOW, DIST_FWD=1.6,
	    STOPPING_FORCE=60, INCREM=.2);

Notice that the order is not important if the parameters are named.

The following routine is a library routine to grasp things.  Basically it
does the following:
.BEGIN FILL; INDENT 8,12
	(1) Optionally open to an OPENING_BEFORE_DEPARTURE.

	(2) Depart via a DEPARTURE (if there is one ... a SPECIAL_DEPARTURE
	    can be specified).

	(3) Start opening the fingers to the OPENING_FOR_APPROACH at the 
	    DEPARTURE point (if SPECIAL_DEPARTURE is specified, use it.
	    Otherwise, use the standard DEPROACH value.).

	(4) Approach the GRASPING_POINT via the APPROACH (if a
	    SPECIAL_APPROACH is specified, use it).

	(5) Center on the object. (If the fingers close so that the opening is
	    less than (THICKNESS - .10) call the operator and give her one
	    chance to re-position the object and try again.)

	(6) Upon successfully centering on the GRASP_POINT, update the
	    OBJECT's position by assigning the GRASP_POINT the current hand
	    location  (this, of course, assumes that either the GRASP_POINT
	    and the OBJECT are the same frame or that the GRASP_POINT is
	    RIGIDLY attached to the OBJECT).
.END

Notice that this routine can be used by either arm.

ROUTINE GRASP(COMPILE_TIME SPECIAL_DEPARTURE, SPECIAL_APPROACH;
	      FRAME THE_ARM(DEFAULT YELLOW), OBJECT, GRASP_POINT,
		  THING_OBJECT_ATTACHED_TO;
	      SCALAR OPENING_BEFORE_DEPARTURE,
		  OPENING_FOR_APPROACH(DEFAULT 15),
		  THICKNESS(DEFAULT .11));
    BEGIN "GRASP"
.COMT 8
    COMMENT SPECIAL_DEPARTURE is a frame expression for the relative
		position of departure.
	    SPECIAL_APPROACH is a frame expression for the relative
		position of the approach.
	    THING_OBJECT_ATTACHED_TO is the name of the frame OBJECT
		is attached to (if there is one) before the GRASP
		routine is called.  It is used to specify what the
		OBJECT should be detached from upon being GRASPed.
	    THICKNESS is defaulted to .11 so that the ON monitor
		ON OPENING < (THICKNESS - .10) DO ... will do a
		reasonable thing;
.END

    COMPILE_TIME T, U, THE_FINGERS;
    COMPILE IF THE_ARM = BLUE
	THEN THE_FINGERS α← ⊂BFINGERS⊃
	ELSE THE_FINGERS α← ⊂YFINGERS⊃;
.COMT 8
	COMMENT This sets up the compile-time variable THE_FINGERS
	    to expand into the correct fingers expression for the
	    OPERATE statements (depending upon the choice of arm);
.END
    COMPILE IF SPECIFIED(OPENING_BEFORE_DEPARTURE) THEN
	OPERATE α#(THE_FINGERS) WITH OPENING=OPENING_BEFORE_DEPARTURE;

.COMT 8
    COMMENT the next statement sets up a compile-time variable,
	U, which contains the phrase "USING APPROACH = <the special>"
	or NIL depending upon whether or not a special approach
	has been specified.  This constructed phrase is used in two
	or three places below to insure that the desired approach is
	being used;
.END
    COMPILE IF SPECIFIED(SPECIAL_APPROACH) 
	THEN U α← ⊂ USING APPROACH = α#(SPECIAL_APPROACH) ⊃
	ELSE U α← ⊂ NIL ⊃;
    COMPILE IF SPECIFIED(SPECIAL_DEPARTURE)
	THEN MOVE THE_ARM TO GRASP_POINT
		USING DEPARTURE=NIL
		VIA THE_ARM * α#(SPECIAL_DEPARTURE) THEN
		    BEGIN
		    OPERATE α#(THE_FINGERS)
			WITH OPENING=OPENING_FOR_APPROACH
		    END
		α#(U)
	ELSE MOVE THE_ARM TO GRASP_POINT
		USING DEPARTURE=NIL
		VIA THE_ARM * THE_DEPARTURE(GRASP_POINT) THEN
		    BEGIN
		    OPERATE α#(THE_FINGERS)
			WITH OPENING=OPENING_FOR_APPROACH
		    END
		α#(U);

    CENTER THE_ARM
	ON OPENING < (THICKNESS-.10) DO
	    BEGIN "MISSED OBJECT"
	    STOP THE_ARM;
	    SCALAR FLAG;
	    OPERATE THE_FINGERS WITH OPENING=OPENING_FOR_APPROACH;
	    COMPILE IF SPECIFIED(SPECIAL_APPROACH)
		THEN BEGIN "MOVE TO SPECIAL APPROACH POINT"
			MOVE THE_ARM TO THE_ARM * α#(SPECIAL_APPROACH)
			    USING APPROACH=NIL, DEPARTURE=NIL;
		     END "MOVE TO SPECIAL APPROACH POINT"
		ELSE BEGIN "USE THE NORMAL APPROACH"
			MOVE THE_ARM
			    TO THE_ARM * THE_APPROACH(GRASP_POINT)
			    DIRECTLY;
		     END "USE THE NORMAL APPROACH";
	    WRITE("GRASP FAILED ... TYPE A `1' TO RETRY");
	    READ(FLAG);
.COMT 16
	        COMMENT this is simply "wait for proceed";
.END
	    IF FLAG %7≠%* 1 THEN ABORT;
	    MOVE THE_ARM TO GRASP_POINT
		USING DEPARTURE=NIL,APPROACH=NIL;
	    CENTER THE_ARM
		ON OPENING < (THICKNESS-.10) DO ABORT ("CLOSED ON AIR");
	    END "MISSED OBJECT";

    ASSERT THE_ARM = GRASP_POINT;
    GRASP_POINT α← THE_ARM;
    COMPILE IF SPECIFIED(THING_OBJECT_ATTACHED_TO) THEN
	DETACH OBJECT FROM THING_OBJECT_ATTACHED_TO;
    ATTACH OBJECT TO THE_ARM;
    END "GRASP";

The following is a typical call on such a routine:
	GRASP(THE_ARM=YELLOW, OBJECT=BRACKET,
		GRASP_POINT=BRACKET_GRASP,
		SPECIAL_APPROACH=⊂ [(0,0,-3):(0,0,0)] ⊃,
		OPENING_FOR_APPROACH=1);

which expands into:

	MOVE YELLOW TO BRACKET_GRASP
	    USING DEPARTURE=NIL
	    VIA YELLOW * [(0,0,10):(0,0,0)] THEN
		BEGIN
	        OPERATE YFINGERS WITH OPENING=1
		END
	    USING APPROACH = [(0,0,-3):(0,0,0)];
	CENTER YELLOW
	    ON OPENING < .01 DO
		BEGIN "MISSED OBJECT"
		STOP YELLOW;
		SCALAR FLAG;
		OPERATE YFINGERS WITH OPENING=1;
		MOVE YELLOW TO YELLOW*[(0,0,-3);(0,0,0)]
		    USING APPROACH=NIL,DEPARTURE=NIL;
		WRITE("GRASP FAILED ... TYPE A `1' TO RETRY");
		READ(FLAG);
		IF FLAG %7≠%* 1 THEN ABORT;
		MOVE YELLOW TO BRACKET_GRASP
		    USING APPROACH=NIL,DEPARTURE=NIL;
		CENTER YELLOW
		    ON OPENING < .01 DO ABORT ("CLOSED ON AIR");
		END "MISSED OBJECT";
	ASSERT YELLOW = BRACKET_GRASP;
	BRACKET_GRASP α← YELLOW;
	ATTACH BRACKET TO YELLOW;

.FILL
Finally, the whole task is  made into a library routine so  it can be
`called' (ie. expanded) as a subtask from a higher level task.
.NOFILL

ROUTINE BOLT_ON_BRACKET;
    BEGIN "WHOLE TASK"
    COMPILE IF α#(YELLOW) %7≠%* YPARK THEN
	COMPILE_ERROR("THE YELLOW ARM IS NOT PLANNED TO BE IN ITS
	    PARK POSITION ... AS ASSUMED BY THE ROUTINE
	    BOLT_ON_BRACKET");
    COMPILE IF (ATTACHED ANYTHING YELLOW) THEN
	COMPILE_ERROR("SOMETHING IS ATTACHED TO THE YELLOW HAND ...
	    AND THE ROUTINE BOLT_ON_BRACKET EXPECTS THE
	    HAND TO BE EMPTY");
.COMT 8
	COMMENT this type of compile time check and warning to the
	    user is very useful for insuring that the interface
	    assumptions for routines are met in the planning
	    world just before the routine is expanded.  Notice that
	    there is a built-in procedure, COMPILE_ERROR, which prints
	    the included message at compile-time and stops the compilation.
	    There is also a compile-time WRITE statement, MESSAGE(" ...").
	    These two different `output' statements are used so that the
	    user can generate WRITE statements within a COMPILE IF.;
.END

    COBEGIN
	BEGIN "PICK UP BRACKET WITH YELLOW"
	GRASP(GRASP_POINT=BRACKET_GRASP, OBJECT=BRACKET,
	    OPENING_FOR_APPROACH=1);
	MOVE BRACKET_HOLE TO BEAM_HOLE + (0, 0, 1.3) WRT BEAM_HOLE;
	MOVE YELLOW TO ⊗ + (0, 0, .5) WRT BEAM_HOLE
	    ON FORCE(Z WRT BEAM_HOLE) > 50 DO STOP YELLOW
	    ON ARRIVAL DO ABORT("I SEEM TO HAVE GONE TOO FAR");
	END "PICK UP BRACKET WITH YELLOW;

	BEGIN "PICK UP BOLT WITH BLUE"
	GRASP(THE_ARM=BLUE, OBJECT=BOLT, GRASP_POINT=BOLT,
	    OPENING_FOR_APPROACH=1)
	END "PICK UP BOLT WITH BLUE"
    COEND;

    MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE;
    NORMAL_SEARCH(BLUE, .2, 1.6, 60, 9);
.COMT 8
	COMMENT assume that the bolt is now in the hole;
.END
    MOVE BLUE TO ⊗ * [(0, 0, 4):(0, 0, 90)]
	ON FORCE(Z WRT BLUE) > 60 DO STOP BLUE;

    COBEGIN "DISENGAGE"
	BEGIN "YELLOW"
	OPERATE YFINGERS WITH OPENING = 1;
	DETACH BRACKET FROM YELLOW;
	ATTACH BRACKET TO BEAM;
	MOVE YELLOW TO YPARK
	END "YELLOW";

	BEGIN "BLUE"
	OPERATE BFINGERS WITH OPENING = 1;
	DETACH BOLT FROM BLUE;
	ATTACH BOLT TO BEAM;
	MOVE BLUE TO BPARK
	END "BLUE"
    COEND "DISENGAGE";
END "WHOLE TASK";

.NEWSS EXAMPLES OF COORDINATED ACTION, COORDINATED ACTION

.FILL
These two examples take into account some  of the more subtle aspects
of  assembly such as freeing  the BRACKET while trying  to insert the
bolt in the hole and changing the speed of the driver dynamically.

The following section of code is designed to simultaneously free the
YELLOW arm and move the BLUE arm to insert the bolt.  The freeing of
the YELLOW arm is to allow the BRACKET to accommodate slightly along the
surface of the BEAM as the BLUE arm tries to insert the bolt.
.NOFILL

MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE;
.COMT 4
    COMMENT remember that the BOLT is in the BLUE hand;
.END
MOVE YELLOW TO ⊗
    USING FREE=(X WRT BEAM_HOLE), FREE=(Y WRT BEAM_HOLE)
    ON DURATION > 0 DO
	BEGIN "INSERTION"
.COMT 12
		COMMENT notice that "DURATION > 0" is an
		    approximation to simultaneous motions;
.END
	NORMAL_SEARCH(BLUE, .2, 1.6, 60, 9);
.COMT 12
		    COMMENT assume that the bolt is now in the hole;
.END
	MOVE BLUE TO ⊗ * [(0, 0, 4):(0, 0, 90)]
	    ON FORCE(Z WRT BLUE) > 60 DO STOP YELLOW;
	END "INSERTION";
    ON DURATION > 4 DO ABORT("OPERATION TOOK TOO LONG");
.COMT 8
	COMMENT the "ON DURATION > 4 DO ABORT" will generate an error
	    if the insertion takes more than 4 seconds.  The error
	    will force the operator to deal with the situation at
	    supervisor level;
.END

Without the SEARCH this could be accomplished in "weak" synchrony:

MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE;
MOVE α{BLUE, YELLOWα}
    TO α{⊗ + (0, 0, 1.6) WRT BLUE, ⊗α}
    USING FREE = α{*, (X WRT BEAM_HOLE)α}
    USING FREE = α{*, (Y WRT BEAM_HOLE)α}
    ON α{FORCE(Z WRT BLUE) > 60, *α} DO α{STOP, STOPα};

.FILL

It is awkward to include  the SEARCH in such a scheme.  In fact, this
type  of coordination  comes up  in a  number of  other places.   For
example, if you want to operate a device (eg. the DRIVER) and move an
arm or camera "at the same time." Events and synchronizing primitives
have  been  added to  solve  these  control problems.    Consider the
following way of programming this task:

.NOFILL

EVENT Y_READY, B_READY;
.COMT 4
	COMMENT Y_READY is an event signalling that the YELLOW
	    arm is ready to move.  B_READY indicates that the
	    BLUE arm is ready to move;
.END
MOVE BOLT TO BEAM_HOLE + (0, 0, -5.3) WRT BEAM_HOLE;
COBEGIN "INSERT THE BOLT"
    BEGIN "FREE YELLOW"
    SIGNAL Y_READY;
    WAIT B_READY;
    MOVE YELLOW TO ⊗
	USING FREE=(X WRT BEAM_HOLE)
	USING FREE=(Y WRT BEAM_HOLE)
	ON DURATION > 4 DO ABORT("TOOK TOO LONG");
    END "FREE YELLOW"

    BEGIN "USE BLUE TO INSERT BOLT"
    SIGNAL B_READY;
    WAIT Y_READY;
    NORMAL_SEARCH(BLUE, .2, 1.6, 60, 9);
.COMT 8
	COMMENT assume that the bolt is now in the hole;
.END
    MOVE BLUE TO ⊗ * [(0, 0, 4):(0, 0, 90)]
	ON FORCE(Z WRT BLUE) > 60 DO STOP YELLOW;
    END "USE BLUE TO INSERT BOLT";
COEND "INSERT THE BOLT";

.FILL

Consider the problem of inserting in a screw and checking to make sure
that it  does not bind.   If, after a short time,  the screw does not
bind, the speed of the DRIVER can be increased.  However, if  it does
bind, everything should stop and the DRIVER should be reversed to try
to unbind the screw.

.NOFILL

EVENT D_READY, B_READY;
SCALAR SP, FLAG;
SP α← 30;
FLAG α← 1;
WHILE FLAG DO
    BEGIN "SCREW LOOP"
    COBEGIN "MOVE AND SCREW SIMULTANEOUSLY"
	BEGIN "OPERATE THE SCREWDRIVER"
	SIGNAL D_READY;
	WAIT B_READY;
	OPERATE DRIVER
	    WITH VELOCITY = SP
	    ON DURATION > 8 DO ABORT("TOOK TOO LONG");
	END "OPERATE THE SCREWDRIVER"

	BEGIN "EXERT DOWNWARD FORCE"
	SIGNAL B_READY;
	WAIT D_READY;
	MOVE BLUE TO ⊗
	    USING FREE=(Z WRT BLUE)
	    USING FORCE=((0, 0, 40) WRT BLUE)
	    "BIND" ON TORQUE(Z WRT BLUE) > 80 DO
		BEGIN "IT BOUND"
		DISABLE "CATCH_OK";
		STOP BLUE;
		STOP DRIVER;
		COBEGIN "TRY TO UNBIND BY REVERSING THE DRIVER"
		    BEGIN "UNSCREW"
		    SIGNAL D_READY;
		    WAIT B_READY;
		    SP α← -60;
		    OPERATE DRIVER
			WITH VELOCITY = SP
			ON DURATION > 4 DO ABORT("CANT UNBIND");
		    END "UNSCREW"

		    BEGIN "UPWARD FORCE"
		    SIGNAL B_READY;
		    WAIT D_READY;
		    MOVE BLUE TO ⊗
			USING FREE=(Z WRT BLUE)
			USING FORCE=((0, 0, -40) WRT BLUE)
			"OUT_OK" ON FORCE(Z WRT BLUE)<20 DO
			    BEGIN
			    STOP DRIVER;
			    STOP BLUE;
.COMT 32
				COMMENT leave FLAG true for retry;
.END
			    END
			"TOO_MUCH_TIME" ON DURATION>4 DO ABORT;
		    END "UPWARD FORCE"
	        COEND "TRY TO UNBIND BY REVERSING THE DRIVER"
		END "IT BOUND"
	    "CATCH_OK" ON DURATION > 1 DO
		BEGIN
		DISABLE "BIND";
		ENABLE "TORQUED_IN_OK";
		SP α← 60;  COMMENT maybe this should be CRITICAL;
		END;
	    DEFER "TORQUED_IN_OK" ON TORQUE(Z WRT BLUE) > 80 DO
		BEGIN
		STOP DRIVER;
		STOP BLUE;
		FLAG α← 0;  COMMENT indicating no retry;
		END;
	END "EXERT DOWNWARD FORCE"
    COEND "MOVE AND SCREW SIMULTANEOUSLY";
    END "SCREW LOOP";

.NEWSS A α`VERY HIGH LEVELα' EXAMPLE

.FILL
This very short example demontrates the use of assembly oriented
special primitives to simplify a task specification, as well as
some of the object description conventions used by those primitives.
Here, the task is the same as that of {sssref ex1}.  For a fuller
explanation of the use of such primitives and another, longer
example, see {secref vhl}.
.NOFILL

FRAME beam,bracket,bolt;
FRAME bracket_bore,beam_bore;
FRAME bolt_grasp,bracket_handle;

.comt 0
α{ We must first describe the various components.  We expect that
eventually the process of making such descriptions will become
very largely automated, as computer programs begin to play an
increasingly active role in mechanical design. See {ssref obd}α }
.end

ASSERT FACT (TYPE beam OBJECT);
ASSERT FACT (GEOMED beam "BEAM.B3Dα[HAL,HEα]"); α{Shape descriptionα}
ASSERT FACT (SUBPART beam beam_bore);
ATTACH beam_bore TO beam RIGIDLY AT α[(0,1.5,6)|(0,π/2,0)α];

ASSERT FACT (TYPE bracket OBJECT);
ASSERT FACT (GEOMED bracket "BRACK.B3Dα[HAL,HEα]"); α{Shape descriptionα}
ASSERT FACT (SUBPART bracket bracket_bore);
ASSERT FACT (SUBPART bracket bracket_handle);
ATTACH bracket_bore TO bracket RIGIDLY AT α[(5.1,2,0)|(π,0,0)α];
ATTACH bracket_handle TO bracket RIGIDLY AT α[(0,0,0)|(π,0,0)α];

ASSERT FACT (TYPE bolt SHAFT);
ASSERT FACT (DIAMETER bolt 0.5);
ASSERT FACT (TOP_END bolt head_type1);
ASSERT FACT (BOTTOM_END bolt tiptype1);
ASSERT FACT (TYPE tiptype1 FLAT_END);

ASSERT FACT (TYPE bracket_bore BORE);
ASSERT FACT (DIAMETER bracket_bore 0.502);
ASSERT FACT (LENGTH bracket_bore 0.5);
ASSERT FACT (TOP_END beacket_bore bracket_hole1);
ASSERT FACT (BOTTOM_END beacket_bore bracket_hole1);

.comt 4
α{et ceteraα}
.end

.comt 0
α{Also, describe how things go togetherα}
.end
ASSERT FACT (TYPE beam_assembly ASSEMBLY);
ASSERT FACT (SUBPART beam_assembly beam);
ASSERT FACT (SUBPART beam_assembly bolt);
ASSERT FACT (SUBPART beam_assembly bracket);

ASSERT FACT (bracket FITS_ONTO beam_assembly AT α[(5.1,2,0):(0,π/2,0)α]);
ASSERT FACT (bolt FITS_ONTO beam_assembly AT α[(5.1,2.3,0):(0,π/2,0)α]);

ASSERT FACT (MATED beam_hsurf bracket_bottom);
ASSERT FACT (ALIGNED beam_bore bracket_bore);
ASSERT FACT (RUNS_THRU bolt bracket_bore);
ASSERT FACT (RUNS_THRU bolt beam_bore);
.COMT 4
α{et ceteraα}
.END

.comt 0
α{Now, describe the initial scene.  Here, assume that the initial object
locations are known preciselyα}
.end
bracket α← α[(20,40,0):(0,0,0)α]
beam α← α[(10,60,0):(0,0,0)α]
bolt α← α[(30,50,5):(0,π,0)α]

GRASP bracket AT α[(0,0,2)|(0,π,0)α] WITH YELLOW;
.comt 0
α{The system will use its internal model of the bracket to
fill in the expected hand opening.α}
.end

FIT bracket ONTO beam_assembly
	USING YELLOW
	AFTERWARDS HOLD bracket WITH YELLOW;

.comt 0
α{The system will use the object description information to fill
in the exact location to which to move the bracket.  Also, it
will pick appropriate techniques to ensure that the bracket
is appropriately aligned.  The AFTERWARDS clause tells the
system that it is to use the yellow arm to hold the bracket in placeα}
.end

INSERT bolt INTO bracket_hole1 USING BLUE;

.comt 0
α{Once again, the system will fill in the details, such as how
the bolt is to be grasped, how it should be brought 
to the hole, how it will be pushed in, and so forth.α}
.end

RELEASE bracket; %4α{ Since the bolt now holds it onα}%*


.FILL